/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/

#include <libxml/parser.h>
#include <libxml/tree.h>
#include "World.h"
#include "Wall.h"
#include "Terrain.h"
#include "J2B2.h"
#include "Avant.h"
#include "Gavant.h"
#include "Marekbot.h"
#include "Skywalker.h"
#include "TestObject.h"
#include "LandMine.h"
#include "SimCtrlObject.h"
#include "AGV.h"
#include "MottiPbV.h"
#include "Car.h"
#include "Ceilbot.h"
#include "Obstacle.h"
#include "Obstacle2.h"
#include "A635Sensors.h"
 
bool loadTerrain(CWorld* world, const xmlNode *start_node)
{
  xmlNode* node=start_node->children;

  char f[1000];
  char n[1000];
  
  while (node!=NULL)
  {
    if ((!xmlStrcmp(node->name,(xmlChar*)"terrain")) )
    {
      if (xmlHasProp(node,(xmlChar *)"file"))
      {
        sscanf( (const char *)xmlGetProp(node,(xmlChar *)"file"), "%s" ,f );
        sscanf( (const char *)xmlGetProp(node,(xmlChar *)"name"), "%s" ,n );
        std::string name(n);
        dPrint(1,"Creating terrain");
        world->SetTerrain(new CTerrain(world,f,name,false));
        return true;
      } else {
        dPrint(1,"MLMA-error: terrain does not name a file!");
        return false;
      }
      
    }
    node=node->next;
  }
  return false;
}

void loadWalls(CWorld* world, const xmlNode *start_node, bool hasTerrain)
{
  xmlNode* node=start_node->children;
  xmlNode* walls_node=NULL;

  //dPrint(1,"Loading walls...\n");

  float x1,y1,x2,y2,defaultW,defaultH;//,width,height,;
  float minx=1000000, maxx=-1000000, miny=1000000, maxy=-1000000;

  while (node!=NULL)
  {
    if ((!xmlStrcmp(node->name,(xmlChar*)"walls")) )
    {
      sscanf( (const char *)xmlGetProp(node,(xmlChar *)"default_width"), "%f" ,&defaultW );
      sscanf( (const char *)xmlGetProp(node,(xmlChar *)"default_height"), "%f" ,&defaultH );
      //dPrint(1,"Wall defaults, width=%f, height=%f.\n",defaultW,defaultH);
      world->wallWidth=defaultW;
      world->wallHeight=defaultH;
      walls_node=node;
      break;
    }
    node=node->next;
  }

  if (walls_node==NULL)
    return;

  node=walls_node->children;
  bool hasWalls=false;
  while (node!=NULL)
  {
    if (!xmlStrcmp(node->name,(xmlChar*)"wall") )
    {
      sscanf( (const char *)xmlGetProp(node,(xmlChar *)"x1"), "%f" ,&x1 );
      sscanf( (const char *)xmlGetProp(node,(xmlChar *)"y1"), "%f" ,&y1 );
      sscanf( (const char *)xmlGetProp(node,(xmlChar *)"x2"), "%f" ,&x2 );
      sscanf( (const char *)xmlGetProp(node,(xmlChar *)"y2"), "%f" ,&y2 );
      //dPrint(1,"Wall, x1=%f x2=%f y1=%f y2=%f\n",x1,x2,y1,y2);
      world->InsertObject(new CWall(world, x1,y1,x2,y2,defaultW,defaultH));

      hasWalls=true;

      if (x1<minx) minx=x1;
      if (x1>maxx) maxx=x1;

      if (x2<minx) minx=x2;
      if (x2>maxx) maxx=x2;

      if (y1<miny) miny=y1;
      if (y1>maxy) maxy=y1;

      if (y2<miny) miny=y2;
      if (y2>maxy) maxy=y2;

    }
    node=node->next;
  }

  if (hasWalls && !hasTerrain)
  {
    world->InsertObject(new CTerrain(world,minx-2,maxx+2,miny-2,maxy+2));
  }

}

void loadObjects(CWorld* world, const xmlNode *start_node)
{
  xmlNode* node=start_node->children;
  xmlNode* objects_node=NULL;

  //dPrint(1,"Loading objects...\n");

  float x,y,heading;

  while (node!=NULL)
  {
    if ((!xmlStrcmp(node->name,(xmlChar*)"objects")) )
    {
      objects_node=node;
      break;
    }
    node=node->next;
  }
  if (objects_node==NULL)
    return;

  node=objects_node->children;
  while (node!=NULL)
  {
    if ((!xmlStrcmp(node->name,(xmlChar*)"J2B2")) )
    {
      CJ2B2::LoadMLMA(world,node);
      /*
      char n[1000];
      //char g[1000];
      TJ2B2Params params;
      sscanf( (const char *)xmlGetProp(node,(xmlChar *)"x"), "%f" ,&x );
      sscanf( (const char *)xmlGetProp(node,(xmlChar *)"y"), "%f" ,&y );
      sscanf( (const char *)xmlGetProp(node,(xmlChar *)"heading"), "%f" ,&heading );
      sscanf( (const char *)xmlGetProp(node,(xmlChar *)"name"), "%s" ,n );
      
      if (xmlGetProp(node,(xmlChar *)"camera"))
      {
        if (xmlStrcmp(xmlGetProp(node,(xmlChar *)"camera"),(xmlChar*)"enabled")
            || xmlStrcmp(xmlGetProp(node,(xmlChar *)"camera"),(xmlChar*)"enable"))
          params.camera=true;
      }
      
      std::string name(n);
      world->InsertObject(new CJ2B2(world, x,y,heading,name,params));
      */
      //dPrint(1,"Loaded robot %s",n);
    }else if ((!xmlStrcmp(node->name,(xmlChar*)"Gavant")) ) {
      char n[1000];
      
      sscanf( (const char *)xmlGetProp(node,(xmlChar *)"x"), "%f" ,&x );
      sscanf( (const char *)xmlGetProp(node,(xmlChar *)"y"), "%f" ,&y );
      sscanf( (const char *)xmlGetProp(node,(xmlChar *)"heading"), "%f" ,&heading );
      sscanf( (const char *)xmlGetProp(node,(xmlChar *)"name"), "%s" ,n );
     
      std::string name(n);
     
      world->InsertObject(new CGavant(world, x,y,heading,name));
    } else if ((!xmlStrcmp(node->name,(xmlChar*)"Avant")) ) {
      char n[1000];
      char m[1000];
 
      sscanf( (const char *)xmlGetProp(node,(xmlChar *)"x"), "%f" ,&x );
      sscanf( (const char *)xmlGetProp(node,(xmlChar *)"y"), "%f" ,&y );
      sscanf( (const char *)xmlGetProp(node,(xmlChar *)"heading"), "%f" ,&heading );
      sscanf( (const char *)xmlGetProp(node,(xmlChar *)"name"), "%s" ,n );
      sscanf( (const char *)xmlGetProp(node,(xmlChar *)"MaCIGroup"), "%s" ,m );
      std::string name(n);
      std::string maciGroup(m);
      world->InsertObject(new CAvant(world, x,y,heading,name,maciGroup));
    } else if ( (!xmlStrcmp(node->name,(xmlChar*)"Marekbot")) || 
                (!xmlStrcmp(node->name,(xmlChar*)"Marsubot")) ) {
      CMarekbot::LoadMLMA(world,node);
      
    } else if ( (!xmlStrcmp(node->name,(xmlChar*)"Skywalker")) ) {
      CSkywalker::LoadMLMA(world,node);
      
    } else if ( (!xmlStrcmp(node->name,(xmlChar*)"LandMine")) ) {
      CLandMine::LoadMLMA(world, node);

    } else if ( (!xmlStrcmp(node->name,(xmlChar*)"SimCtrlObject")) ) {
      CSimCtrlObject::LoadMLMA(world, node);

    } else if ( (!xmlStrcmp(node->name,(xmlChar*)"MottiPbV")) ) {
      CMottiPbV::LoadMLMA(world, node);

    } else if ( (!xmlStrcmp(node->name,(xmlChar*)"Ceilbot")) ) {
      CCeilbot::LoadMLMA(world, node);

    } else if ( (!xmlStrcmp(node->name,(xmlChar*)"TestObject")) ) {
      CTestObject::LoadMLMA(world,node);

    } else if ( (!xmlStrcmp(node->name,(xmlChar*)"AGV")) ) {
      CAGV::LoadMLMA(world,node);

    }else if ( (!xmlStrcmp(node->name,(xmlChar*)"A635Sensors")) ) {
      CA635Sensors::LoadMLMA(world,node);

    } 
		else if ( (!xmlStrcmp(node->name,(xmlChar*)"Car")) ) {
      CCar::LoadMLMA(world,node);

    } else if ( (!xmlStrcmp(node->name,(xmlChar*)"Obstacle")) ) {
      CObstacle::LoadMLMA(world,node);

    } else if ( (!xmlStrcmp(node->name,(xmlChar*)"Obstacle2")) ) {
      CObstacle2::LoadMLMA(world,node);

    }
    node=node->next;
  }

}

CWorld* MLMALoadWorld(const char* filename)
{
  xmlDoc *doc = NULL;
  xmlNode *root_element = NULL;

  /*
  * this initialize the library and check potential ABI mismatches
  * between the version it was compiled for and the actual shared
  * library used.
  */
  LIBXML_TEST_VERSION

  /*parse the file and get the DOM */
  //printf("trying to read file...\n");
  doc = xmlReadFile(filename, NULL, 0);

  if (doc == NULL) {
    dPrint(1,"MLMA-error: Could not parse file %s\n", filename);
    return NULL;
  }
  //printf("file parsed...\n");

  /*Get the root element node */
  root_element = xmlDocGetRootElement(doc);

  if (xmlStrcmp(root_element->name,(xmlChar*)"world"))
  {
    dPrint(1,"MLMA-error: File %s doesn't seem to be a world-file.\n", filename);
    return NULL;
  }
  
  dPrint(1,"Creating world");
  CWorld *world=new CWorld;


  //Read the base MaCI Group for World//////////////
  std::string MaCIGroup = "FSRSim";
  char name[256];
  xmlNode* node=root_element->children;
  while (node!=NULL)
  {
    if ((!xmlStrcmp(node->name,(xmlChar*)"BaseMaCIGroup")) )
    {
   
      sscanf( ( char *)xmlGetProp(node,(xmlChar *)"name"), "%s" ,name );
      MaCIGroup = name;
      break;
    }
   
    node=node->next;
  }

  world->SetBaseMaCIGroup(MaCIGroup);
  //////////////////////////////////////////////////

  bool terrain=loadTerrain(world, root_element);
  loadWalls(world, root_element,terrain);
  loadObjects(world, root_element);

  return world;
}

